#include "defaults.h"
#include "nurbs.h"
#include "utils.h"
#include "nurbspatch.h"
#include "primitivemanager.h"

math::Vec2ui Index1Dto2D(size_t k, size_t n) {
	size_t i = k % n;
	size_t j = (k - i) / n;
	return math::V2ui(i, j);
}

size_t Index2Dto1D(const math::Vec2ui& index, size_t n) {
	return index[1] * n + index[0];
}

size_t Index2Dto1D(size_t i, size_t j, size_t n) {
	return j * n + i;
}

SNurbs::SNurbs(const boost::multi_array<Node*, 2>& c, const Knotvector& U, const Knotvector& V, size_t p, size_t q, Attribute& attribute, size_t id) : ElementImplementation<SNurbs>(attribute, id), c(c), U(U), V(V), p(p), q(q) {
	domain = std::pair<Vec2n, Vec2n>(math::V2n(U[p], U[p + 1]), math::V2n(V[q], V[q + 1]));
}

bool SNurbs::Init() {
	EvaluateBasis();

	for(size_t i = 0; i <= p; i++) {
		for(size_t j = 0; j <= q; j++) {
			Node* v = GetNode(i, j);
			size_t gid = v->Id();
			size_t lid = Index2Dto1D(i, j, p + 1);
			mapGlobalToLocal[gid] = lid;
		}
	}
}

nfloat SNurbs::BasisFun(const Vec2n& param, const math::Vec2ui& k) const {
	nfloat bu = math::spline::BasisFun(param[0], k[0], p, U);
	nfloat bv = math::spline::BasisFun(param[1], k[1], q, V);
	return bu * bv;
}

nfloat SNurbs::BasisFun(const size_t i, const size_t j, const std::vector<nfloat>& bu, const std::vector<nfloat>& bv) const {
	return bu[i] * bv[j];
}
	
nfloat SNurbs::BasisFun(const math::Vec2ui& index, const math::Vec2ui& D, const math::Vec2ui& k) const {
	const nfloat& bu = B[0][D[0]][k[0]][index[0]];
	const nfloat& bv = B[1][D[1]][k[1]][index[1]];
	return bu * bv;
}

void SNurbs::EvaluateBasis() {
 	const math::Vec2ui n = math::V2ui(2 * p + 1, 2 * q + 1);
	
	const math::Vec2ui degree = math::V2ui(p, q);
	std::vector<Knotvector> kv(2);
	std::vector<Vec2n> d(2);
	d[0] = domain.first;
	d[1] = domain.second;
	kv[0] = U;
	kv[1] = V;

	std::vector<Vec2n> s(2);
	for(size_t j = 0; j < 2; j++) {
		const Vec2n& dj = d[j];
		s[j][0] = (dj[1] - dj[0]) * 0.5;
		s[j][1] = 0.5 * (dj[0] + dj[1]);
	}
	const nfloat d0 = s[0][0] * s[1][0];

//  B[dim][basis_degree][index][sample]
	B.resize(2);
	for(size_t i = 0; i < B.size(); i++) {
		B[i].resize(degree[i] + 1);
		for(size_t j = 0; j < B[i].size(); j++) {
			B[i][j].resize(degree[i] + 1);
			for(size_t k = 0; k < B[i][j].size(); k++)
				B[i][j][k].resize(n[i]);
		}
	}

	parameter.resize(2);
	for(size_t i = 0; i < 2; i++) {
		std::vector< std::pair<nfloat, nfloat> > g;
		gauleg(-1.0, 1.0, g, n[i]);

		const nfloat& a0 = s[i][0];
		const nfloat& a1 = s[i][1];
		
		for(size_t l = 0; l < n[i]; l++) {
			const nfloat uk = a0 * g[l].first + a1;
			parameter[i].push_back(uk);
			const std::vector< std::vector<nfloat> > Basis = math::spline::DersBasisFuns<nfloat>(degree[i], uk, degree[i], degree[i], kv[i]);

			//Basis[basis_degree][index]
			for(size_t j = 0; j <= degree[i]; j++)
				for(size_t k = 0; k < degree[i] + 1; k++)
					B[i][j][k][l] = Basis[j][k];
		}

		std::vector<nfloat> wi(n[i]);
		for(size_t j = 0; j < n[i]; j++) {
			wi[j] = std::sqrt(d0) * g[j].second;
		}
		qweight[i] = wi;
	}
}

const Node* SNurbs::GetNode(size_t i) const {
	math::Vec2ui k = Index1Dto2D(i, p + 1);
	return c[k[0]][k[1]];
}

Node* SNurbs::GetNode(size_t i) {
	math::Vec2ui k = Index1Dto2D(i, p + 1);
	return c[k[0]][k[1]];
}

const Node* SNurbs::GetNode(size_t i, size_t j) const {
	return c[i][j];
}

Node* SNurbs::GetNode(size_t i, size_t j) {
	return c[i][j];
}

math::Vec2ui SNurbs::LocalIndex(const Node* v) const {
	size_t lid = mapGlobalToLocal[v->Id()];
	return Index1Dto2D(lid, p + 1);
}

static bool Within(const nfloat& x, const Vec2n& domain) {
	return domain[0] < x && x < domain[1];
}

bool SNurbs::InDomain(const Vec2n& X) const {
	const Vec2n& u = domain.first;
	const Vec2n& v = domain.second;
	const nfloat x = X[0];
	const nfloat y = X[1];

	const nfloat eps = 0.0001;

	if(Within(x, u) && Within(y, v))
		return true;

	if( (std::abs(x - u[0]) < eps && Within(y, v)) ||
		(std::abs(x - u[1]) < eps && Within(y, v)) ||
		(std::abs(y - v[0]) < eps && Within(x, u)) ||
		(std::abs(y - v[1]) < eps && Within(x, u)) ||

		(std::abs(x - u[0]) < eps && std::abs(y - v[0]) < eps) ||
		(std::abs(x - u[0]) < eps && std::abs(y - v[1]) < eps) ||
		(std::abs(x - u[1]) < eps && std::abs(y - v[0]) < eps) ||
		(std::abs(x - u[1]) < eps && std::abs(y - v[1]) < eps))
		return true;

 	return false;
}

Vec3n SNurbs::Normal(const Vec2n& x, const std::string& attribute) const {
	const std::vector< std::vector<nfloat> > BasisU = math::spline::DersBasisFuns<nfloat>(p, x[0], p, p, U);
	const std::vector< std::vector<nfloat> > BasisV = math::spline::DersBasisFuns<nfloat>(q, x[1], q, q, V);

	std::vector<nfloat> bu0(p + 1), bv0(q + 1);
	for(size_t i = 0; i <= p; i++) bu0[i] = BasisU[0][i];
	for(size_t i = 0; i <= q; i++) bv0[i] = BasisV[0][i];
	
	std::vector<nfloat> bu1(p + 1), bv1(q + 1);
	for(size_t i = 0; i <= p; i++) bu1[i] = BasisU[1][i];
	for(size_t i = 0; i <= q; i++) bv1[i] = BasisV[1][i];

	if(Exist("weight")) {
		size_t weightHandle = AttributeHandle("weight");
		size_t positionHandle = AttributeHandle(attribute);

		Vec4n S = math::V4n(0.0, 0.0, 0.0, 0.0);
		Vec4n u = math::V4n(0.0, 0.0, 0.0, 0.0);
		Vec4n v = math::V4n(0.0, 0.0, 0.0, 0.0);
		for(size_t i = 0; i <= p; i++) {
			for(size_t j = 0; j <= q; j++) {
				const nfloat& weight = GetNode(i, j)->Get<nfloat>(weightHandle);
				const Vec3n& position = GetNode(i, j)->Get<Vec3n>(positionHandle);
				Vec4n P = math::V4n(weight * position, weight);

				S.MulAdd(P, BasisFun(i, j, bu0, bv0));
				u.MulAdd(P, BasisFun(i, j, bu1, bv0));
				v.MulAdd(P, BasisFun(i, j, bu0, bv1));
			}
		}
		
		Vec3n n = math::V3n(S[0], S[1], S[2]);
		nfloat d = S[3];
	
		Vec3n n_ = math::V3n(u[0], u[1], u[2]);
		nfloat d_ = u[3];
		Vec3n U2 = (1.0 / (d * d)) * (d * n_ - d_ * n);
		
		n_ = math::V3n(v[0], v[1], v[2]);
		d_ = v[3];
		Vec3n V2 = (1.0 / (d * d)) * (d * n_ - d_ * n);
		
		Vec3n normal = cross(U2, V2);
		normalize(normal);
	
		return normal;
	} else {
		Vec3n u = math::V3n(0.0, 0.0, 0.0);
		Vec3n v = math::V3n(0.0, 0.0, 0.0);

		size_t positionHandle = AttributeHandle(attribute);
		for(size_t i = 0; i <= p; i++) {
			for(size_t j = 0; j <= q; j++) {
				const Vec3n& position = GetNode(i, j)->Get<Vec3n>(positionHandle);

				u.MulAdd(position, BasisFun(i, j, bu1, bv0));
				v.MulAdd(position, BasisFun(i, j, bu0, bv1));
			}
		}

		Vec3n normal = cross(u, v);
		normalize(normal);

		return normal;
	}
}

Vec2n SNurbs::Grad(const Vec2n& param, const math::Vec2ui& k, bool useWeights) const {
	if(useWeights) {
		std::vector<nfloat> bu0(p + 1), bv0(q + 1);
		std::vector<nfloat> bu1(p + 1), bv1(q + 1);
		for (size_t i = 0; i <= p; i++) bu0[i] = math::spline::BasisFun<nfloat>(param[0], i, p, U);
		for (size_t i = 0; i <= q; i++) bv0[i] = math::spline::BasisFun<nfloat>(param[1], i, q, V);
		for (size_t i = 0; i <= p; i++) bu1[i] = math::spline::BasisFunDer<nfloat>(param[0], i, p, U);
		for (size_t i = 0; i <= q; i++) bv1[i] = math::spline::BasisFunDer<nfloat>(param[1], i, q, V);

		nfloat w = 0.0;
		Vec2n gradw = math::V2n(0.0, 0.0);

		for(size_t i = 0; i <= p; i++) {
			for(size_t j = 0; j <= q; j++) {
				nfloat weight = GetNode(i, j)->Get<nfloat>("weight");
			
				w += weight * bu0[i] * bv0[j];
				gradw[0] += weight * bu1[i] * bv0[j];
				gradw[1] += weight * bu0[i] * bv1[j];
			}
		}

		const nfloat& b0 = bu0[k[0]];
		const nfloat& b1 = bv0[k[1]];
		const nfloat& d0 = bu1[k[0]];
		const nfloat& d1 = bv1[k[1]];
	
		Vec2n grad;
		grad[0] = d0 * b1 * w - b0 * b1 * gradw[0];
		grad[1] = b0 * d1 * w - b0 * b1 * gradw[1];
	
		return (GetNode(k[0], k[1])->Get<nfloat>("weight") / (w * w)) * grad;
	} else {
		nfloat b0 = math::spline::BasisFun(param[0], k[0], p, U);
		nfloat b1 = math::spline::BasisFun(param[1], k[1], q, V);
		nfloat d0 = math::spline::BasisFunDer(param[0], k[0], p, U);
		nfloat d1 = math::spline::BasisFunDer(param[1], k[1], q, V);
		Vec2n grad = math::V2n(d0 * b1, b0 * d1);

		return grad;
	}
}

Vec2n SNurbs::Grad(const math::Vec2ui& index, const math::Vec2ui& k, bool useWeights) const {
	if(useWeights) {
		std::vector<nfloat> bu0(p + 1), bv0(q + 1);
		std::vector<nfloat> bu1(p + 1), bv1(q + 1);
		for (size_t i = 0; i <= p; i++) bu0[i] = BasisFun(index[0], 0, 0, i);
		for (size_t i = 0; i <= q; i++) bv0[i] = BasisFun(index[1], 1, 0, i);
		for (size_t i = 0; i <= p; i++) bu1[i] = BasisFun(index[0], 0, 1, i);
		for (size_t i = 0; i <= q; i++) bv1[i] = BasisFun(index[1], 1, 1, i);

		nfloat w = 0.0;
		Vec2n gradw = math::V2n(0.0, 0.0);

		for(size_t i = 0; i <= p; i++) {
			for(size_t j = 0; j <= q; j++) {
				nfloat weight = GetNode(i, j)->Get<nfloat>("weight");
			
				w += weight * bu0[i] * bv0[j];
				gradw[0] += weight * bu1[i] * bv0[j];
				gradw[1] += weight * bu0[i] * bv1[j];
			}
		}

		const nfloat& b0 = bu0[k[0]];
		const nfloat& b1 = bv0[k[1]];
		const nfloat& d0 = bu1[k[0]];
		const nfloat& d1 = bv1[k[1]];
	
		Vec2n grad;
		grad[0] = d0 * b1 * w - b0 * b1 * gradw[0];
		grad[1] = b0 * d1 * w - b0 * b1 * gradw[1];
	
		return (GetNode(k[0], k[1])->Get<nfloat>("weight") / (w * w)) * grad;
	} else {
		const nfloat gu = BasisFun(index, math::V2ui(1, 0), k);
		const nfloat gv = BasisFun(index, math::V2ui(0, 1), k);
		return math::V2n(gu, gv);
	}

	return math::V2n(0.0, 0.0);
}

Matrix SNurbs::Jacobian(const Vec2n& param, const std::string& attribute, bool useWeights) const {
	std::vector<nfloat> bu0(p + 1), bv0(q + 1);
	std::vector<nfloat> bu1(p + 1), bv1(q + 1);
	for (size_t i = 0; i <= p; i++) bu0[i] = math::spline::BasisFun<nfloat>(param[0], i, p, U);
	for (size_t i = 0; i <= q; i++) bv0[i] = math::spline::BasisFun<nfloat>(param[1], i, q, V);
	for (size_t i = 0; i <= p; i++) bu1[i] = math::spline::BasisFunDer<nfloat>(param[0], i, p, U);
	for (size_t i = 0; i <= q; i++) bv1[i] = math::spline::BasisFunDer<nfloat>(param[1], i, q, V);

	Matrix A(3, 2);
	if(useWeights) {
		Vec4n S = math::V4n(0.0, 0.0, 0.0, 0.0);
		Vec4n u = math::V4n(0.0, 0.0, 0.0, 0.0);
		Vec4n v = math::V4n(0.0, 0.0, 0.0, 0.0);
		for(size_t i = 0; i <= p; i++) {
			for(size_t j = 0; j <= q; j++) {
				const nfloat& weight = GetNode(i, j)->Get<nfloat>("weight");
				const Vec3n& position = GetNode(i, j)->Get<Vec3n>(attribute);
				Vec4n P = math::V4n(weight * position, weight);

				
				S.MulAdd(P, BasisFun(i, j, bu0, bv0));
				u.MulAdd(P, BasisFun(i, j, bu1, bv0));
				v.MulAdd(P, BasisFun(i, j, bu0, bv1));
			}
		}
	
		Vec3n n = math::V3n(S[0], S[1], S[2]);
		nfloat d = S[3];
	
		Vec3n n_ = math::V3n(u[0], u[1], u[2]);
		nfloat d_ = u[3];
		Vec3n U2 = (1.0 / (d * d)) * (d * n_ - d_ * n);
	
		n_ = math::V3n(v[0], v[1], v[2]);
		d_ = v[3];
		Vec3n V2 = (1.0 / (d * d)) * (d * n_ - d_ * n);
	
		A(0, 0) = u[0]; A(0, 1) = v[0];
		A(1, 0) = u[1]; A(1, 1) = v[1];
		A(2, 0) = u[2]; A(2, 1) = v[2];
	} else {
		Vec3n u = math::V3n(0.0, 0.0, 0.0);
		Vec3n v = math::V3n(0.0, 0.0, 0.0);
		for(size_t i = 0; i <= p; i++) {
			for(size_t j = 0; j <= q; j++) {
				const Vec3n& P = GetNode(i, j)->Get<Vec3n>(attribute);

				u.MulAdd(P, BasisFun(i, j, bu1, bv0));
				v.MulAdd(P, BasisFun(i, j, bu0, bv1));
			}
		}
		
		A(0, 0) = u[0]; A(0, 1) = v[0];
		A(1, 0) = u[1]; A(1, 1) = v[1];
		A(2, 0) = u[2]; A(2, 1) = v[2];
	}

	return A;
}
	
Matrix SNurbs::Jacobian(const math::Vec2ui& index, const std::string& attribute, const bool useWeights) const {
	Matrix A(3, 2);
	if(useWeights) {
		Vec4n S = math::V4n(0.0, 0.0, 0.0, 0.0);
		Vec4n u = math::V4n(0.0, 0.0, 0.0, 0.0);
		Vec4n v = math::V4n(0.0, 0.0, 0.0, 0.0);
		for(size_t i = 0; i <= p; i++) {
			for(size_t j = 0; j <= q; j++) {
				const nfloat& weight = GetNode(i, j)->Get<nfloat>("weight");
				const Vec3n& position = GetNode(i, j)->Get<Vec3n>(attribute);
				const Vec4n P = math::V4n(weight * position, weight);

				const nfloat bu0bv0 = BasisFun(index, math::V2ui(0, 0), math::V2ui(i, j));
				const nfloat bu1bv0 = BasisFun(index, math::V2ui(1, 0), math::V2ui(i, j));
				const nfloat bu0bv1 = BasisFun(index, math::V2ui(0, 1), math::V2ui(i, j));
				S.MulAdd(P, bu0bv0);
				u.MulAdd(P, bu1bv0);
				v.MulAdd(P, bu0bv1);
			}
		}
	
		Vec3n n = math::V3n(S[0], S[1], S[2]);
		nfloat d = S[3];
	
		Vec3n n_ = math::V3n(u[0], u[1], u[2]);
		nfloat d_ = u[3];
		Vec3n U2 = (1.0 / (d * d)) * (d * n_ - d_ * n);
	
		n_ = math::V3n(v[0], v[1], v[2]);
		d_ = v[3];
		Vec3n V2 = (1.0 / (d * d)) * (d * n_ - d_ * n);
	
		A(0, 0) = u[0]; A(0, 1) = v[0];
		A(1, 0) = u[1]; A(1, 1) = v[1];
		A(2, 0) = u[2]; A(2, 1) = v[2];
	} else {
		Vec3n u = math::V3n(0.0, 0.0, 0.0);
		Vec3n v = math::V3n(0.0, 0.0, 0.0);
		for(size_t i = 0; i <= p; i++) {
			for(size_t j = 0; j <= q; j++) {
				const Vec3n& P = GetNode(i, j)->Get<Vec3n>(attribute);

				const nfloat bu1bv0 = BasisFun(index, math::V2ui(1, 0), math::V2ui(i, j));
				const nfloat bu0bv1 = BasisFun(index, math::V2ui(0, 1), math::V2ui(i, j));
				u.MulAdd(P, bu1bv0);
				v.MulAdd(P, bu0bv1);
			}
		}

		A(0, 0) = u[0]; A(0, 1) = v[0];
		A(1, 0) = u[1]; A(1, 1) = v[1];
		A(2, 0) = u[2]; A(2, 1) = v[2];
	}
	return A;
}

boost::multi_array<nfloat, 2> SNurbs::QuadraturePoints() const {
	std::vector< std::pair<nfloat, nfloat> > g;
	
	const size_t n = 5;
	gauleg(-1.0, 1.0, g, n);
	
	std::pair<Vec2n, Vec2n> domain = Domain();
	const nfloat& a = domain.first[0];
	const nfloat& b = domain.first[1];
	const nfloat& c = domain.second[0];
	const nfloat& d = domain.second[1];
	
	nfloat a0 = (b - a) * 0.5;
	nfloat a1 = 0.5 * (a + b);
	nfloat b0 = (d - c) * 0.5;
	nfloat b1 = 0.5 * (c + d);
	nfloat c0 = a0 * b0;
	
	boost::multi_array<nfloat, 2> gqp(boost::extents[n][3]);
	for(int i = 0; i < n; i++) {
		gqp[i][0] = a0 * g[i].first + a1; //u
		gqp[i][1] = b0 * g[i].first + b1; //v
		gqp[i][2] = std::sqrt(c0) * g[i].second; //weight
	}
	
	return gqp;
}

#include "Mathematica.h"

static Vec3n Phi(const Vec3n& Su, const Vec3n& Sv, const Vec2n& b) {
	const nfloat& xu = Su[0];
	const nfloat& yu = Su[1];
	const nfloat& zu = Su[2];
	const nfloat& xv = Sv[0];
	const nfloat& yv = Sv[1];
	const nfloat& zv = Sv[2];
	const nfloat& bu = b[0];
	const nfloat& bv = b[1];

	const nfloat det = 1.0 / (Squared(xv)*(Squared(yu) + Squared(zu)) + Squared(yv*zu - yu*zv) - 2*xu*xv*(yu*yv + zu*zv) + Squared(xu)*(Squared(yv) + Squared(zv)));

	return det * math::V3n(bv*(xv*(Squared(yu) + Squared(zu)) - xu*(yu*yv + zu*zv)) + bu*(-(xv*(yu*yv + zu*zv)) + xu*(Squared(yv) + Squared(zv))),
						   bv*(-(xu*xv*yu) + Squared(xu)*yv + zu*(yv*zu - yu*zv)) + bu*(Squared(xv)*yu - xu*xv*yv + zv*(-(yv*zu) + yu*zv)),
						   bu*(Squared(xv)*zu - xu*xv*zv + yv*(yv*zu - yu*zv)) + bv*(-(xu*xv*zu) + Squared(xu)*zv + yu*(-(yv*zu) + yu*zv)));
}

nfloat SNurbs::Stiffness(const math::Vec2ui& k0, const math::Vec2ui& k1, const std::string& attribute, bool useWeights) const {
	nfloat s = 0.0;
	for(size_t i = 0; i < GetSamplesU(); i++) {
		for(size_t j = 0; j < GetSamplesV(); j++) {
			const math::Vec2ui index = math::V2ui(i, j);
			const nfloat weight = Weight(index);

			const Matrix J = Jacobian(index, attribute, useWeights);
			const Vec3n Su = math::V3n(J(0, 0), J(1, 0), J(2, 0));
			const Vec3n Sv = math::V3n(J(0, 1), J(1, 1), J(2, 1));

			const Vec2n bi = Grad(index, k0, useWeights);
			const Vec2n bj = Grad(index, k1, useWeights);
			const Vec3n phii = Phi(Su, Sv, bi);
			const Vec3n phij = Phi(Su, Sv, bj);

			const Vec3n SuxSv = cross(Su, Sv);
			const nfloat Jdet = length(SuxSv);

			s += weight * Jdet * dot(phii, phij);
		}
	}

	return s;
}

nfloat SNurbs::Mass(const math::Vec2ui& k0, const math::Vec2ui& k1, const std::string& attribute, bool useWeights) const {
	nfloat m = 0.0;
	for(size_t i = 0; i < GetSamplesU(); i++) {
		for(size_t j = 0; j < GetSamplesV(); j++) {
			const math::Vec2ui index = math::V2ui(i, j);
			const nfloat weight = Weight(index);
			
			const Matrix J = Jacobian(index, attribute, useWeights);
			const Vec3n u = math::V3n(J(0, 0), J(1, 0), J(2, 0));
			const Vec3n v = math::V3n(J(0, 1), J(1, 1), J(2, 1));
			const Vec3n uxv = cross(u, v);
			const nfloat Jdet = length(uxv);

			m += weight * Jdet * BasisFun(index, math::V2ui(0, 0), k0) * BasisFun(index, math::V2ui(0, 0), k1);
		}
	}
	
	return m;
}

Matrix SNurbs::Stiffness(const std::string& attribute) const {
	bool useWeights = Exist("weight");

	const size_t n = (p + 1) * (q + 1);
	Matrix K(n, n);
	for(size_t i = 0; i < n; i++) {
		for(size_t j = 0; j < n; j++) {
			math::Vec2ui i0 = Index1Dto2D(i, p + 1);
			math::Vec2ui i1 = Index1Dto2D(j, p + 1);
			K(i, j) = Stiffness(i0, i1, attribute, useWeights);
		}
	}
	return -1.0 * K;
}

Matrix SNurbs::Mass(const std::string& attribute, const bool lumped) const {
	bool useWeights = Exist("weight");

	const size_t n = (p + 1) * (q + 1);
	Matrix M = ZeroMatrix(n, n);
	if(lumped) {
		for(size_t i = 0; i < n; i++) {
			nfloat sum = 0.0;
			for(size_t j = 0; j < n; j++) {
				math::Vec2ui i0 = Index1Dto2D(i, p + 1);
				math::Vec2ui i1 = Index1Dto2D(j, p + 1);
				sum += Mass(i0, i1, attribute, useWeights);
			}
			M(i, i) = sum;
		}
	} else {
		for(size_t i = 0; i < n; i++) {
			for(size_t j = 0; j < n; j++) {
				math::Vec2ui i0 = Index1Dto2D(i, p + 1);
				math::Vec2ui i1 = Index1Dto2D(j, p + 1);
				M(i, j) = Mass(i0, i1, attribute, useWeights);
			}
		}
	}
	return M;
}

Vector SNurbs::Force(const std::string& attribute) const {
 	size_t attributeHandle = AttributeHandle(attribute);
	const size_t n = (p + 1) * (q + 1);
	Vector b(n);
	for(size_t i = 0; i < n; i++) {
		math::Vec2ui k = Index1Dto2D(i, p + 1);
		b(i) = Integrate<nfloat>(GetNode(k[0], k[1]), attributeHandle);
	}

	return b;
}

std::pair< std::vector<Vec3n>, std::vector<math::Vec3ui> > SNurbs::Tesselate(const size_t eppU, const size_t eppV, const std::string& attribute) const {
	const std::pair<Vec2n, Vec2n>& domain = Domain();

	std::vector<nfloat> u(eppU);
	const Vec2n& uDomain = domain.first;
	for(size_t i = 0; i < eppU; i++) {
		const nfloat t = (nfloat)i / (eppU - 1);
		u[i] = t * (uDomain[1] - uDomain[0]) + uDomain[0];
	}

	std::vector<nfloat> v(eppV);
	const Vec2n& vDomain = domain.second;
	for(size_t i = 0; i < eppV; i++) {
		const nfloat t = (nfloat)i / (eppV - 1);
		v[i] = t * (vDomain[1] - vDomain[0]) + vDomain[0];
	}

	boost::multi_array<Vec2n, 2> param(boost::extents[eppU][eppV]);
	for(size_t i = 0; i < eppU; i++) {
		for(size_t j = 0; j < eppV; j++) {
			param[i][j] = math::V2n(u[i], v[j]);
		}
	}

	return Tesselate(param, attribute);
}

std::pair< std::vector<Vec3n>, std::vector<math::Vec3ui> > SNurbs::Tesselate(const Array2V2n& param, const std::string& attribute) const {
	const size_t n = param.shape()[0];
	const size_t m = param.shape()[1];

	boost::multi_array<Vec3n, 2> A(boost::extents[n][m]);
	for(size_t i = 0; i < n; i++) {
		for(size_t j = 0; j < m; j++) {
			A[i][j] = Evaluate<Vec3n>(param[i][j], attribute);
		}
	}

	std::map<std::string, size_t> mapPosition;
	std::vector<Vec3n> position;
	std::vector<math::Vec3ui> index;
	for(size_t i = 0; i < n - 1; i++) {
		for(size_t j = 0; j < m - 1; j++) {
			const size_t i00 = GetIndex<nfloat>(A[i][j], mapPosition, position);
			const size_t i01 = GetIndex<nfloat>(A[i][j + 1], mapPosition, position);
			const size_t i11 = GetIndex<nfloat>(A[i + 1][j + 1], mapPosition, position);
			const size_t i10 = GetIndex<nfloat>(A[i + 1][j], mapPosition, position);
			index.push_back(math::V3ui(i00, i01, i11));
			index.push_back(math::V3ui(i00, i11, i10));
		}
	}

	return std::pair< std::vector<Vec3n>, std::vector<math::Vec3ui> >(position, index);
}
